﻿using Microsoft.Kinect;
using Microsoft.Kinect.Toolkit.FaceTracking;
using System;
using System.Diagnostics;
using System.Windows;
using System.Windows.Media;

namespace FaceTrackerAMEE.Model
{
    /// <summary>
    /// El Bruno's Face Tracker (modified).<para>
    /// http://elbruno.com/2012/08/08/kinect-howto-use-face-recognition-with-kinectsdk-ii/
    /// </para>
    /// </summary>
    public class SkeletonFaceTracker : IDisposable
    {
        #region Fields

        FaceTracker _faceTracker;
        SkeletonTrackingState _skeletonTrackingState;

        /// <summary>The 3D coordinate of the tracked eye.</summary>
        Vector3DF _eyeVec;
        /// <summary>The 2D projected coordinate of the tracked eye.</summary>
        PointF _eyeProjected;
        /// <summary>The single Feature point definition for camera posing (InnerBottomRightPupil)</summary>
        static readonly FeaturePoint _featurePoint = FeaturePoint.InnerCornerRightEye;// FeaturePoint.InnerBottomRightPupil;
        static readonly double _radiusXY = 4.0;
        /// <summary>???</summary>
        private DateTime _startFaceRecognition;

        #endregion

        #region Properties

        /// <summary>
        /// Gets or sets the computed face tracking results for an image frame.
        /// </summary>
        /// <value>
        /// The last tracked frame.
        /// </value>
        public int LastTrackedFrame { get; set; }

        /// <summary>Indicates if the last face track succeeded</summary>
        public bool LastFaceTrackSucceeded;

        /// <summary>The 3D coordinate of the tracked eye.</summary>
        public Vector3DF Eye3D { get { return _eyeVec; } }

        /// <summary>The 2D projected coordinate of the tracked eye.</summary>
        public PointF _Eye2D { get { return _eyeProjected; } }
        

        #endregion

        #region Constructor

        /// <summary>
        /// Initializes a new instance of the <see cref="SkeletonFaceTracker"/> class.
        /// </summary>
        public SkeletonFaceTracker() {
        }

        #endregion

        #region Public

        /// <summary>
        /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources.
        /// </summary>
        public void Dispose() {
            if (_faceTracker == null) return;
            _faceTracker.Dispose();
            _faceTracker = null;
        }

        /// <summary>
        /// Draws the point of interest.
        /// </summary>
        /// <param name="drawingContext">The drawing context.</param>
        /// <remarks>
        /// Called by the overriden FaceTrackerView.OnRender().
        /// </remarks>
        public void DrawPointOfInterest(DrawingContext drawingContext) {
            // do some timinng !?
            if (!LastFaceTrackSucceeded || _skeletonTrackingState != SkeletonTrackingState.Tracked) {
                _startFaceRecognition = DateTime.MinValue;
                return;
            }

            // draw tracked Point
            Pen myPen = new Pen() { Thickness = 10, LineJoin = PenLineJoin.Round, EndLineCap = PenLineCap.Round };
            System.Windows.Point center = new System.Windows.Point(_eyeProjected.X + 0.5f, _eyeProjected.Y + 0.5f);
            //double radiusXY = 4.0;
            drawingContext.DrawEllipse(
                Brushes.Red,
                myPen,
                center,
                _radiusXY,
                _radiusXY);

            // do more timing !?!
            if (_startFaceRecognition == DateTime.MinValue) {
                _startFaceRecognition = DateTime.Now;
            }
            else {
                if (DateTime.Now.Subtract(_startFaceRecognition).TotalSeconds > 5) {
                    Application.Current.Dispatcher.BeginInvoke(new Action(() => {
                        System.Threading.Thread.Sleep(300);
                        _startFaceRecognition = DateTime.MinValue;
                    }));
                }
            }
        }

         

        #endregion

        /// <summary>
        /// Updates the face tracking information for this skeleton
        /// </summary>
        /// <param name="kinectSensor">The kinect sensor.</param>
        /// <param name="colorImageFormat">The color image format.</param>
        /// <param name="colorImage">The color image.</param>
        /// <param name="depthImageFormat">The depth image format.</param>
        /// <param name="depthImage">The depth image.</param>
        /// <param name="skeletonOfInterest">The skeleton of interest.</param>
        internal void OnFrameReady(KinectSensor kinectSensor, ColorImageFormat colorImageFormat, byte[] colorImage, DepthImageFormat depthImageFormat, short[] depthImage, Skeleton skeletonOfInterest) {
            _skeletonTrackingState = skeletonOfInterest.TrackingState;
            if (_skeletonTrackingState != SkeletonTrackingState.Tracked) {
                return;
            }

            // erzeuge neuen FaceTracker, falls noch nicht vorhanden
            if (_faceTracker == null) {
                try {
                    _faceTracker = new FaceTracker(kinectSensor);
                }
                catch (InvalidOperationException) {
                    Debug.WriteLine("AllFramesReady - creating a new FaceTracker threw an InvalidOperationException", "Warning SkeletonFaceTracker");
                    _faceTracker = null;
                }
            }
            if (_faceTracker == null) 
                return;

            // Start face tracking from Kinect input data
            var frame = _faceTracker.Track(colorImageFormat, colorImage, depthImageFormat, depthImage, skeletonOfInterest);
            LastFaceTrackSucceeded = frame.TrackSuccessful; // Gesicht gefunden ?
            if (!LastFaceTrackSucceeded)
                return;
            //Debug.WriteLine("Track Successfull", "Info SkeletonFaceTracker");
            var shape2D = frame.GetProjected3DShape();
            _eyeProjected = shape2D[_featurePoint];
            var shape3D = frame.Get3DShape();
            _eyeVec = shape3D[FeaturePoint.InnerBottomRightPupil];
            //Debug.WriteLine(String.Format("Found right eye at (x,y,z) = {0:F2}, {1:F2}, {2:F2}", _eyeVec.X, _eyeVec.Y, _eyeVec.Z), "Info SkeletonFaceTracker");

            //if (_faceTriangles == null) {
            //    _faceTriangles = frame.GetTriangles();
            //}
            //_facePoints = frame.GetProjected3DShape();
        }

    }
}
